/**
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
  * @file       chassis_mecanum.c/h
  * @brief      麦轮轮底盘控制器。
  * @note       包括初始化，目标量更新、状态量更新、控制量计算与直接控制量的发送
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-9-2024      Tina_Lin         1. done
  *  V1.0.1     Dec-9-2024      Tina_Lin         1. 完成基本控制
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
*/

#include "robot_param.h"
#if (CHASSIS_TYPE == CHASSIS_TEST)
#include "CAN_receive.h"
#include "chassis.h"
#include "chassis_test.h"
#include "detect_task.h"
#include "gimbal.h"
#include "math.h"
#include "motor.h"
#include "referee.h"
#include "ui.h"
#include "usb_debug.h"
#include "usb_task.h"

Chassis_s CHASSIS;
CHASSIS_PID_t chassis_pid;
extern int ui_self_id = 0;

/*-------------------- Init --------------------*/

/**
 * @brief          初始化
 * @param[in]      none
 * @retval         none
 */
void ChassisInit(void)
{
    CHASSIS.rc = get_remote_control_point();  // 获取遥控器指针
    /*-------------------- 初始化PID --------------------*/
    //速度环pid
    float chassis_speed_pid[3] = {
        KP_CHASSIS_WHEEL_SPEED, KI_CHASSIS_WHEEL_SPEED, KD_CHASSIS_WHEEL_SPEED};

    //获取底盘电机数据指针，初始化PID
    uint8_t i;
    for (i = 0; i < 4; i++) {
        //底盘不跟随云台下的pid初始化
        PID_init(
            &chassis_pid.motor_speed_pid[i], PID_POSITION, chassis_speed_pid,
            MAX_OUT_CHASSIS_WHEEL_SPEED, MAX_IOUT_CHASSIS_WHEEL_SPEED);
    }
    /*-------------------- 初始化底盘电机 --------------------*/
    MotorInit(&CHASSIS.wheel_motor[0], 1, 1, DJI_M3508, 1, 1, 1);
    MotorInit(&CHASSIS.wheel_motor[1], 2, 1, DJI_M3508, 1, 1, 1);
    MotorInit(&CHASSIS.wheel_motor[2], 3, 1, DJI_M3508, 1, 1, 1);
    MotorInit(&CHASSIS.wheel_motor[3], 4, 1, DJI_M3508, 1, 1, 1);
}

/*-------------------- Set mode --------------------*/

/**
 * @brief          设置模式
 * @param[in]      none
 * @retval         none
 */
void ChassisSetMode(void)
{
    CHASSIS.last_mode = CHASSIS.mode;
    if (switch_is_up(CHASSIS.rc->rc.s[CHASSIS_MODE_CHANNEL])) {
        CHASSIS.mode = CHASSIS_SPIN;
    } else if (switch_is_mid(CHASSIS.rc->rc.s[CHASSIS_MODE_CHANNEL])) {
        CHASSIS.mode = CHASSIS_FREE;
    } else if (switch_is_down(CHASSIS.rc->rc.s[CHASSIS_MODE_CHANNEL])) {
        CHASSIS.mode = CHASSIS_ZERO_FORCE;
    }
}

/*-------------------- Observe --------------------*/

/**
 * @brief          更新状态量
 * @param[in]      none
 * @retval         none
 */
void ChassisObserver(void)
{
    for (uint8_t i = 0; i < 4; i++) {
        GetMotorMeasure(&CHASSIS.wheel_motor[i]);
    }
}

/*-------------------- Reference --------------------*/



/**
 * @brief          更新目标量
 * @param[in]      none
 * @retval         none
 */
void ChassisReference(void)
{
    fp32 rc_x, rc_y, rc_z;
    fp32 vx_channel, vy_channel, wz_channel;
    rc_deadband_limit(CHASSIS.rc->rc.ch[3], rc_x, CHASSIS_RC_DEADLINE);
    rc_deadband_limit(CHASSIS.rc->rc.ch[2], rc_y, CHASSIS_RC_DEADLINE);
    rc_deadband_limit(CHASSIS.rc->rc.ch[0], rc_z, CHASSIS_RC_DEADLINE);

    vx_channel = rc_x * MAX_SPEED_VECTOR_VX;
    vy_channel = rc_y * MAX_SPEED_VECTOR_VY;
    wz_channel = rc_z * MAX_SPEED_VECTOR_WZ;

    CHASSIS.vx_rc_set = vx_channel;
    CHASSIS.vy_rc_set = vy_channel;
    CHASSIS.wz_rc_set = wz_channel;

    CHASSIS.dyaw = GetGimbalDeltaYawMid();

    //具体模式设定
    switch (CHASSIS.mode) {
        case CHASSIS_ZERO_FORCE: {
            CHASSIS.wz_set = CHASSIA_STOP_SPEED;
            CHASSIS.vx_set = CHASSIA_STOP_SPEED;
            CHASSIS.vy_set = CHASSIA_STOP_SPEED;
            break;
        }
        case CHASSIS_FOLLOW_GIMBAL_YAW:  //云台跟随模式
        {
            fp32 sin_yaw = 0.0f, cos_yaw = 0.0f;
            // 控制vx vy
            sin_yaw = sinf(CHASSIS.dyaw);
            cos_yaw = cosf(CHASSIS.dyaw);
            CHASSIS.vy_set = cos_yaw * CHASSIS.vy_rc_set + sin_yaw * CHASSIS.vx_rc_set;
            CHASSIS.vx_set = -sin_yaw * CHASSIS.vy_rc_set + cos_yaw * CHASSIS.vx_rc_set;
            CHASSIS.wz_set = PID_calc(&chassis_pid.chassis_angle_pid, CHASSIS.dyaw, 0);

        } break;
        case CHASSIS_STOP:
            break;
        case CHASSIS_FREE: {                     //底盘不跟随云台
            CHASSIS.wz_set = CHASSIS.wz_rc_set;  //暂时让这个模式下小陀螺不生效
            CHASSIS.vy_set = CHASSIS.vy_rc_set;
            CHASSIS.vx_set = CHASSIS.vx_rc_set;
            break;
        }
        case CHASSIS_SPIN:  //小陀螺模式
        {
            fp32 sin_yaw = 0.0f, cos_yaw = 0.0f;
            // 控制vx vy
            sin_yaw = sinf(CHASSIS.dyaw);
            cos_yaw = cosf(CHASSIS.dyaw);
            CHASSIS.vy_set = cos_yaw * CHASSIS.vy_rc_set + sin_yaw * CHASSIS.vx_rc_set;   //vx
            CHASSIS.vx_set = -sin_yaw * CHASSIS.vy_rc_set + cos_yaw * CHASSIS.vx_rc_set;  //vy
            CHASSIS.wz_set = 300.0f;
            break;
        }
        case CHASSIS_AUTO:
            break;
        case CHASSIS_OPEN:

            break;
    }
}
/*-------------------- Console --------------------*/

/**
 * @brief          计算控制量
 * @param[in]      none
 * @retval         none
 */
void ChassisConsole(void)
{
    uint8_t i;

    // 判断是否出错，若出错则将电流全部置零
    if (toe_is_error(DBUS_TOE)) {
        for (i = 0; i < 4; i++) {
            CHASSIS.wheel_motor[i].set.curr = CHASSIA_CURR_ZERO;
        }
        return;
    }

    //麦轮解算
    CHASSIS.wheel_motor[0].set.vel = CHASSIS.vx_set + CHASSIS.vy_set + CHASSIS.wz_set;
    CHASSIS.wheel_motor[1].set.vel = CHASSIS.vx_set - CHASSIS.vy_set + CHASSIS.wz_set;
    CHASSIS.wheel_motor[2].set.vel = -CHASSIS.vx_set + CHASSIS.vy_set + CHASSIS.wz_set;
    CHASSIS.wheel_motor[3].set.vel = -CHASSIS.vx_set - CHASSIS.vy_set + CHASSIS.wz_set;

    //pid速度计算
    for (i = 0; i < 4; i++) {
        CHASSIS.wheel_motor[i].set.curr = PID_calc(
            &chassis_pid.motor_speed_pid[i], CHASSIS.wheel_motor[i].fdb.vel,
            CHASSIS.wheel_motor[i].set.vel);
    }
}

/*-------------------- Cmd --------------------*/

/**
 * @brief          发送控制量
 * @param[in]      none
 * @retval         none
 */

void ChassisSendCmd(void)
{
    CanCmdDjiMotor(
        1, 0x200, CHASSIS.wheel_motor[0].set.curr, CHASSIS.wheel_motor[1].set.curr,
        CHASSIS.wheel_motor[2].set.curr, CHASSIS.wheel_motor[3].set.curr);
}

#endif
